/**
 * @file can_lc.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief LC CAN 发送节点
 * @version 0.1
 * @date 2020-07-13
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "lib_lc.h"

#include "ros/ros.h"
#include "storm_car/can_data.h"

uint16_t g_devicehandle;

/**
 * @brief LC CAN 初始化函数
 * 
 * @return true 初始化成功
 * @return false 初始化失败
 */
bool LCCANInit(void);

/**
 * @brief LC CAN 通道初始化函数
 * 
 * @return true 初始化成功
 * @return false 初始化失败
 */
bool LCCANChanleStart(void);

/**
 * @brief LC CAN 数据发送函数
 * 
 * @param msg CAN_send_data 话题消息
 */
void SendROSDatatoCAN(const storm_car::can_data::ConstPtr& msg);

/**
 * @brief CAN 数据透传节点
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "CAN_LC");

  ROS_INFO("This is CAN_LC");

  ros::NodeHandle CAN;
  ros::Subscriber receive = CAN.subscribe("can_send_data", 1000, SendROSDatatoCAN);
  
  if(0 == LCCANInit())
  {
      return 0;
  }

  if(0 == LCCANChanleStart())
  {
      return 0;
  }
  
  ros::spin();

  return 0;
}

bool LCCANInit(void)
{
  // 打开设备
  if ( (g_devicehandle = CAN_DeviceOpen(ACUSB_132B, 0, 0)) == 0 )
  {
    ROS_INFO("open deivce error\n");
    return false;
  }

  CAN_DeviceInformation DevInfo;
  if ( CAN_GetDeviceInfo(g_devicehandle, &DevInfo) != CAN_RESULT_OK )
  {
    ROS_INFO("GetDeviceInfo error\n");
    return false;
  }
  ROS_INFO("\t--%s--\n", DevInfo.szDescription);
  ROS_INFO("\tSN:%s\n", DevInfo.szSerialNumber);
  ROS_INFO("\tCAN 通道数:%d\n", DevInfo.bChannelNumber);
  ROS_INFO("\t硬件  版本:%x\n", DevInfo.uHardWareVersion);
  ROS_INFO("\t固件  版本:%x\n\n", DevInfo.uFirmWareVersion);
  ROS_INFO("\t驱动  版本:%x\n", DevInfo.uDriverVersion);
  ROS_INFO("\t接口库版本:%x\n", DevInfo.uInterfaceVersion);

  return true;
}

bool LCCANChanleStart(void)
{
  // 启动CAN通道
  CAN_InitConfig config;
  config.dwAccCode = 0;
  config.dwAccMask = 0xffffffff;
  config.nFilter  = 0;        // 滤波方式(0表示未设置滤波功能,1表示双滤波,2表示单滤波)
  config.bMode    = 0;        // 工作模式(0表示正常模式,1表示只听模式)
  config.nBtrType = 1;        // 位定时参数模式(1表示SJA1000,0表示LPC21XX)
  config.dwBtr[0] = 0x00;     // BTR0   0014 -1M 0016-800K 001C-500K 011C-250K 031C-12K 041C-100K 091C-50K 181C-20K 311C-10K BFFF-5K
  config.dwBtr[1] = 0x1c;     // BTR1
  config.dwBtr[2] = 0;
  config.dwBtr[3] = 0;

  if ( CAN_ChannelStart(g_devicehandle, 0, &config) != CAN_RESULT_OK )
  {
    ROS_INFO("Start CAN 0 error!\n");
    return false;
  }
  else
  {
    ROS_INFO("Start CAN 0 success!\n");
  }
  
  if ( CAN_ChannelStart(g_devicehandle, 1, &config) != CAN_RESULT_OK )
  {
    ROS_INFO("Start CAN 1 error!\n");
    return false;
  }
  else{
    ROS_INFO("Start CAN 1 success!\n");
  }

  return true;
}

void SendROSDatatoCAN(const storm_car::can_data::ConstPtr& msg)
{
  CAN_DataFrame tmp;
  tmp.uID = 0x0238;
  tmp.nSendType = 0;
  tmp.bRemoteFlag = 0;
  tmp.bExternFlag = 0;
  tmp.nDataLen = 8;

  for(int i = 0; i < 8; ++ i)
  {
    tmp.arryData[i] = msg->data[i];
  }
  CAN_ChannelSend(g_devicehandle, 0, &tmp, 1);
}